#include "FastOps.h"
#include "Vector.h"
#include "List.h"

namespace RSIM{

namespace FastOps{
        
void spatial_inverse_shift_vec6(const double *f,
                                const double *r,
                                double *m){
        m[0]=f[0]+f[4]*r[2]-f[5]*r[1];
        m[1]=f[1]-f[3]*r[2]+f[5]*r[0];
        m[2]=f[2]+f[3]*r[1]-f[4]*r[0];
        m[3]=f[3];
        m[4]=f[4];
        m[5]=f[5];
}

void mult_mat66_bvec6(const double *I, const double *f, double *M){
        M[0]=I[3]*f[0]+I[4]*f[1]+I[5]*f[2];
        M[1]=I[9]*f[0]+I[10]*f[1]+I[11]*f[2];
        M[2]=I[15]*f[0]+I[16]*f[1]+I[17]*f[2];
        M[3]=I[21]*f[0]+I[22]*f[1]+I[23]*f[2];
        M[4]=I[27]*f[0]+I[28]*f[1]+I[29]*f[2];
        M[5]=I[33]*f[0]+I[34]*f[1]+I[35]*f[2];
}

void mult_mat66_tvec6(const double *I, const double *f, double *M){
        M[0]=I[0]*f[0]+I[1]*f[1]+I[2]*f[2];
        M[1]=I[6]*f[0]+I[7]*f[1]+I[8]*f[2];
        M[2]=I[12]*f[0]+I[13]*f[1]+I[14]*f[2];
        M[3]=I[18]*f[0]+I[19]*f[1]+I[20]*f[2];
        M[4]=I[24]*f[0]+I[25]*f[1]+I[26]*f[2];
        M[5]=I[30]*f[0]+I[31]*f[1]+I[32]*f[2];
}

void spatial_basis_shift_mat(const double *C, const double *A, double *p){
        double t[146];
        
        t[2]=A[0]*C[0];
        t[3]=A[12]*C[2];
        t[4]=A[6]*C[1];
        t[5]=t[2]+t[3]+t[4];
        t[6]=A[1]*C[0];
        t[7]=A[13]*C[2];
        t[8]=A[7]*C[1];
        t[9]=t[6]+t[7]+t[8];
        t[10]=A[2]*C[0];
        t[11]=A[14]*C[2];
        t[12]=A[8]*C[1];
        t[13]=t[10]+t[11]+t[12];
        t[14]=A[10]*C[1];
        t[15]=A[16]*C[2];
        t[16]=A[4]*C[0];
        t[17]=t[14]+t[15]+t[16];
        t[18]=A[11]*C[1];
        t[19]=A[17]*C[2];
        t[20]=A[5]*C[0];
        t[21]=t[18]+t[19]+t[20];
        t[22]=A[3]*C[0];
        t[23]=A[15]*C[2];
        t[24]=A[9]*C[1];
        t[25]=t[22]+t[23]+t[24];
        t[26]=A[0]*C[3];
        t[27]=A[12]*C[5];
        t[28]=A[6]*C[4];
        t[29]=t[26]+t[27]+t[28];
        t[30]=A[1]*C[3];
        t[31]=A[13]*C[5];
        t[32]=A[7]*C[4];
        t[33]=t[30]+t[31]+t[32];
        t[34]=A[2]*C[3];
        t[35]=A[14]*C[5];
        t[36]=A[8]*C[4];
        t[37]=t[34]+t[35]+t[36];
        t[38]=A[10]*C[4];
        t[39]=A[16]*C[5];
        t[40]=A[4]*C[3];
        t[41]=t[38]+t[39]+t[40];
        t[42]=A[11]*C[4];
        t[43]=A[17]*C[5];
        t[44]=A[5]*C[3];
        t[45]=t[42]+t[43]+t[44];
        t[46]=A[3]*C[3];
        t[47]=A[15]*C[5];
        t[48]=A[9]*C[4];
        t[49]=t[46]+t[47]+t[48];
        t[50]=A[0]*C[6];
        t[51]=A[12]*C[8];
        t[52]=A[6]*C[7];
        t[53]=t[50]+t[51]+t[52];
        t[54]=A[1]*C[6];
        t[55]=A[13]*C[8];
        t[56]=A[7]*C[7];
        t[57]=t[54]+t[55]+t[56];
        t[58]=A[2]*C[6];
        t[59]=A[14]*C[8];
        t[60]=A[8]*C[7];
        t[61]=t[58]+t[59]+t[60];
        t[62]=A[10]*C[7];
        t[63]=A[16]*C[8];
        t[64]=A[4]*C[6];
        t[65]=t[62]+t[63]+t[64];
        t[66]=A[11]*C[7];
        t[67]=A[17]*C[8];
        t[68]=A[5]*C[6];
        t[69]=t[66]+t[67]+t[68];
        t[70]=A[3]*C[6];
        t[71]=A[15]*C[8];
        t[72]=A[9]*C[7];
        t[73]=t[70]+t[71]+t[72];
        t[74]=A[18]*C[0];
        t[75]=A[24]*C[1];
        t[76]=A[30]*C[2];
        t[77]=t[74]+t[75]+t[76];
        t[78]=A[19]*C[0];
        t[79]=A[25]*C[1];
        t[80]=A[31]*C[2];
        t[81]=t[78]+t[79]+t[80];
        t[82]=A[20]*C[0];
        t[83]=A[26]*C[1];
        t[84]=A[32]*C[2];
        t[85]=t[82]+t[83]+t[84];
        t[86]=A[21]*C[0];
        t[87]=A[27]*C[1];
        t[88]=A[33]*C[2];
        t[89]=t[86]+t[87]+t[88];
        t[90]=A[22]*C[0];
        t[91]=A[28]*C[1];
        t[92]=A[34]*C[2];
        t[93]=t[90]+t[91]+t[92];
        t[94]=A[23]*C[0];
        t[95]=A[29]*C[1];
        t[96]=A[35]*C[2];
        t[97]=t[94]+t[95]+t[96];
        t[98]=A[18]*C[3];
        t[99]=A[24]*C[4];
        t[100]=A[30]*C[5];
        t[101]=t[100]+t[98]+t[99];
        t[102]=A[19]*C[3];
        t[103]=A[25]*C[4];
        t[104]=A[31]*C[5];
        t[105]=t[102]+t[103]+t[104];
        t[106]=A[20]*C[3];
        t[107]=A[26]*C[4];
        t[108]=A[32]*C[5];
        t[109]=t[106]+t[107]+t[108];
        t[110]=A[21]*C[3];
        t[111]=A[27]*C[4];
        t[112]=A[33]*C[5];
        t[113]=t[110]+t[111]+t[112];
        t[114]=A[22]*C[3];
        t[115]=A[28]*C[4];
        t[116]=A[34]*C[5];
        t[117]=t[114]+t[115]+t[116];
        t[118]=A[23]*C[3];
        t[119]=A[29]*C[4];
        t[120]=A[35]*C[5];
        t[121]=t[118]+t[119]+t[120];
        t[122]=A[18]*C[6];
        t[123]=A[24]*C[7];
        t[124]=A[30]*C[8];
        t[125]=t[122]+t[123]+t[124];
        t[126]=A[19]*C[6];
        t[127]=A[25]*C[7];
        t[128]=A[31]*C[8];
        t[129]=t[126]+t[127]+t[128];
        t[130]=A[20]*C[6];
        t[131]=A[26]*C[7];
        t[132]=A[32]*C[8];
        t[133]=t[130]+t[131]+t[132];
        t[134]=A[21]*C[6];
        t[135]=A[27]*C[7];
        t[136]=A[33]*C[8];
        t[137]=t[134]+t[135]+t[136];
        t[138]=A[22]*C[6];
        t[139]=A[28]*C[7];
        t[140]=A[34]*C[8];
        t[141]=t[138]+t[139]+t[140];
        t[142]=A[23]*C[6];
        t[143]=A[29]*C[7];
        t[144]=A[35]*C[8];
        t[145]=t[142]+t[143]+t[144];
        
        p[0]=C[2]*t[13]+C[0]*t[5]+C[1]*t[9];
        p[1]=C[5]*t[13]+C[3]*t[5]+C[4]*t[9];
        p[2]=C[8]*t[13]+C[6]*t[5]+C[7]*t[9];
        p[3]=C[0]*t[25]+C[1]*t[17]+C[2]*t[21];
        p[4]=C[3]*t[25]+C[4]*t[17]+C[5]*t[21];
        p[5]=C[6]*t[25]+C[7]*t[17]+C[8]*t[21];
        p[6]=C[0]*t[29]+C[1]*t[33]+C[2]*t[37];
        p[7]=C[3]*t[29]+C[4]*t[33]+C[5]*t[37];
        p[8]=C[6]*t[29]+C[7]*t[33]+C[8]*t[37];
        p[9]=C[0]*t[49]+C[1]*t[41]+C[2]*t[45];
        p[10]=C[3]*t[49]+C[4]*t[41]+C[5]*t[45];
        p[11]=C[6]*t[49]+C[7]*t[41]+C[8]*t[45];
        p[12]=C[0]*t[53]+C[1]*t[57]+C[2]*t[61];
        p[13]=C[3]*t[53]+C[4]*t[57]+C[5]*t[61];
        p[14]=C[6]*t[53]+C[7]*t[57]+C[8]*t[61];
        p[15]=C[0]*t[73]+C[1]*t[65]+C[2]*t[69];
        p[16]=C[3]*t[73]+C[4]*t[65]+C[5]*t[69];
        p[17]=C[6]*t[73]+C[7]*t[65]+C[8]*t[69];
        p[18]=C[0]*t[77]+C[1]*t[81]+C[2]*t[85];
        p[19]=C[3]*t[77]+C[4]*t[81]+C[5]*t[85];
        p[20]=C[6]*t[77]+C[7]*t[81]+C[8]*t[85];
        p[21]=C[0]*t[89]+C[1]*t[93]+C[2]*t[97];
        p[22]=C[3]*t[89]+C[4]*t[93]+C[5]*t[97];
        p[23]=C[6]*t[89]+C[7]*t[93]+C[8]*t[97];
        p[24]=C[0]*t[101]+C[1]*t[105]+C[2]*t[109];
        p[25]=C[3]*t[101]+C[4]*t[105]+C[5]*t[109];
        p[26]=C[6]*t[101]+C[7]*t[105]+C[8]*t[109];
        p[27]=C[0]*t[113]+C[1]*t[117]+C[2]*t[121];
        p[28]=C[3]*t[113]+C[4]*t[117]+C[5]*t[121];
        p[29]=C[6]*t[113]+C[7]*t[117]+C[8]*t[121];
        p[30]=C[0]*t[125]+C[1]*t[129]+C[2]*t[133];
        p[31]=C[3]*t[125]+C[4]*t[129]+C[5]*t[133];
        p[32]=C[6]*t[125]+C[7]*t[129]+C[8]*t[133];
        p[33]=C[0]*t[137]+C[1]*t[141]+C[2]*t[145];
        p[34]=C[3]*t[137]+C[4]*t[141]+C[5]*t[145];
        p[35]=C[6]*t[137]+C[7]*t[141]+C[8]*t[145];
}

///////////////////////////////////////////////////////

void basis_shift_mat33(const double *C, const double *A, double *p){
        double t[38];
        t[2]=A[0]*C[0];
        t[3]=A[3]*C[1];
        t[4]=A[6]*C[2];
        t[5]=t[2]+t[3]+t[4];
        t[6]=A[1]*C[0];
        t[7]=A[4]*C[1];
        t[8]=A[7]*C[2];
        t[9]=t[6]+t[7]+t[8];
        t[10]=A[2]*C[0];
        t[11]=A[5]*C[1];
        t[12]=A[8]*C[2];
        t[13]=t[10]+t[11]+t[12];
        t[14]=A[0]*C[3];
        t[15]=A[3]*C[4];
        t[16]=A[6]*C[5];
        t[17]=t[14]+t[15]+t[16];
        t[18]=A[1]*C[3];
        t[19]=A[4]*C[4];
        t[20]=A[7]*C[5];
        t[21]=t[18]+t[19]+t[20];
        t[22]=A[2]*C[3];
        t[23]=A[5]*C[4];
        t[24]=A[8]*C[5];
        t[25]=t[22]+t[23]+t[24];
        t[26]=A[0]*C[6];
        t[27]=A[3]*C[7];
        t[28]=A[6]*C[8];
        t[29]=t[26]+t[27]+t[28];
        t[30]=A[1]*C[6];
        t[31]=A[4]*C[7];
        t[32]=A[7]*C[8];
        t[33]=t[30]+t[31]+t[32];
        t[34]=A[2]*C[6];
        t[35]=A[5]*C[7];
        t[36]=A[8]*C[8];
        t[37]=t[34]+t[35]+t[36];
        p[0]=C[2]*t[13]+C[0]*t[5]+C[1]*t[9];
        p[1]=C[5]*t[13]+C[3]*t[5]+C[4]*t[9];
        p[2]=C[8]*t[13]+C[6]*t[5]+C[7]*t[9];
        p[3]=C[0]*t[17]+C[1]*t[21]+C[2]*t[25];
        p[4]=C[3]*t[17]+C[4]*t[21]+C[5]*t[25];
        p[5]=C[6]*t[17]+C[7]*t[21]+C[8]*t[25];
        p[6]=C[0]*t[29]+C[1]*t[33]+C[2]*t[37];
        p[7]=C[3]*t[29]+C[4]*t[33]+C[5]*t[37];
        p[8]=C[6]*t[29]+C[7]*t[33]+C[8]*t[37];
}

///////////////////////////////////////////////////////

void transpose_basis_shift_mat33(const double *C, const double *A, double *p){
        double t[38];
        t[2]=A[0]*C[0];
        t[3]=A[3]*C[3];
        t[4]=A[6]*C[6];
        t[5]=t[2]+t[3]+t[4];
        t[6]=A[1]*C[0];
        t[7]=A[4]*C[3];
        t[8]=A[7]*C[6];
        t[9]=t[6]+t[7]+t[8];
        t[10]=A[2]*C[0];
        t[11]=A[5]*C[3];
        t[12]=A[8]*C[6];
        t[13]=t[10]+t[11]+t[12];
        t[14]=A[0]*C[1];
        t[15]=A[3]*C[4];
        t[16]=A[6]*C[7];
        t[17]=t[14]+t[15]+t[16];
        t[18]=A[1]*C[1];
        t[19]=A[4]*C[4];
        t[20]=A[7]*C[7];
        t[21]=t[18]+t[19]+t[20];
        t[22]=A[2]*C[1];
        t[23]=A[5]*C[4];
        t[24]=A[8]*C[7];
        t[25]=t[22]+t[23]+t[24];
        t[26]=A[0]*C[2];
        t[27]=A[3]*C[5];
        t[28]=A[6]*C[8];
        t[29]=t[26]+t[27]+t[28];
        t[30]=A[1]*C[2];
        t[31]=A[4]*C[5];
        t[32]=A[7]*C[8];
        t[33]=t[30]+t[31]+t[32];
        t[34]=A[2]*C[2];
        t[35]=A[5]*C[5];
        t[36]=A[8]*C[8];
        t[37]=t[34]+t[35]+t[36];
        p[0]=C[0]*t[5]+C[6]*t[13]+C[3]*t[9];
        p[1]=C[1]*t[5]+C[7]*t[13]+C[4]*t[9];
        p[2]=C[2]*t[5]+C[8]*t[13]+C[5]*t[9];
        p[3]=C[0]*t[17]+C[3]*t[21]+C[6]*t[25];
        p[4]=C[1]*t[17]+C[4]*t[21]+C[7]*t[25];
        p[5]=C[2]*t[17]+C[5]*t[21]+C[8]*t[25];
        p[6]=C[0]*t[29]+C[3]*t[33]+C[6]*t[37];
        p[7]=C[1]*t[29]+C[4]*t[33]+C[7]*t[37];
        p[8]=C[2]*t[29]+C[5]*t[33]+C[8]*t[37];
}

///////////////////////////////////////////////////////

void inv_mat3x3(const double *c, double *p){
        double det = c[0]*c[4]*c[8] - c[0]*c[5]*c[7] - c[1]*c[3]*c[8] + c[1]*c[5]*c[6] + c[2]*c[3]*c[7] - c[2]*c[4]*c[6];
        if(fabs(det)>1.0E-10){
                p[0]=(c[4]*c[8] - c[5]*c[7])/det;
                p[1]=(c[2]*c[7] - c[1]*c[8])/det;
                p[2]=(c[1]*c[5] - c[2]*c[4])/det;
                p[3]=(c[5]*c[6] - c[3]*c[8])/det;
                p[4]=(c[0]*c[8] - c[2]*c[6])/det;
                p[5]=(c[2]*c[3] - c[0]*c[5])/det;
                p[6]=(c[3]*c[7] - c[4]*c[6])/det;
                p[7]=(c[1]*c[6] - c[0]*c[7])/det;
                p[8]=(c[0]*c[4] - c[1]*c[3])/det;
        }
        else{
                cout<<"\ninv_mat3x3(): Matrix is poorly conditioned\n";
                exit(0);
        }
}

///////////////////////////////////////////////////////

void spatial_basis_shift_vec(const double *c, const double *m, double *p){
        p[0]=c[0]*m[0] + c[1]*m[1] + c[2]*m[2];
        p[1]=c[3]*m[0] + c[4]*m[1] + c[5]*m[2];
        p[2]=c[6]*m[0] + c[7]*m[1] + c[8]*m[2];
        p[3]=c[0]*m[3] + c[1]*m[4] + c[2]*m[5];
        p[4]=c[3]*m[3] + c[4]*m[4] + c[5]*m[5];
        p[5]=c[6]*m[3] + c[7]*m[4] + c[8]*m[5];
}

///////////////////////////////////////////////////////

void transpose_mult_mat33_vec3(const double *m, const double *n, double *p){	
        p[0]=m[0]*n[0] + m[3]*n[1] + m[6]*n[2];
        p[1]=m[1]*n[0] + m[4]*n[1] + m[7]*n[2];
        p[2]=m[2]*n[0] + m[5]*n[1] + m[8]*n[2];
}

///////////////////////////////////////////////////////

void wxwxr(const double *w, const double *r, double *wwr){
        wwr[0] = - w[1]*(r[0]*w[1] - r[1]*w[0]) - w[2]*(r[0]*w[2] - r[2]*w[0]);
        wwr[1] = w[0]*(r[0]*w[1] - r[1]*w[0]) - w[2]*(r[1]*w[2] - r[2]*w[1]);
        wwr[2] = w[0]*(r[0]*w[2] - r[2]*w[0]) + w[1]*(r[1]*w[2] - r[2]*w[1]);
}

///////////////////////////////////////////////////////

void cross(const double *a, const double *b, double *c){
        c[0] = a[1]*b[2] - a[2]*b[1];
        c[1] = a[2]*b[0] - a[0]*b[2];
        c[2] = a[0]*b[1] - a[1]*b[0];
}

///////////////////////////////////////////////////////

void mult_mat33_mat33(const double *m1, const double *m2, double *m3){
        m3[0] = m1[0]*m2[0] + m1[1]*m2[3] + m1[2]*m2[6];
        m3[1] = m1[0]*m2[1] + m1[1]*m2[4] + m1[2]*m2[7];
        m3[2] = m1[0]*m2[2] + m1[1]*m2[5] + m1[2]*m2[8];
        
        m3[3] = m1[3]*m2[0] + m1[4]*m2[3] + m1[5]*m2[6];
        m3[4] = m1[3]*m2[1] + m1[4]*m2[4] + m1[5]*m2[7];
        m3[5] = m1[3]*m2[2] + m1[4]*m2[5] + m1[5]*m2[8];
        
        m3[6] = m1[6]*m2[0] + m1[7]*m2[3] + m1[8]*m2[6];
        m3[7] = m1[6]*m2[1] + m1[7]*m2[4] + m1[8]*m2[7];
        m3[8] = m1[6]*m2[2] + m1[7]*m2[5] + m1[8]*m2[8];
}

void mmult_mat33_mat33(const double *m1, const double *m2, double *m3){
        m3[0] = -(m1[0]*m2[0] + m1[1]*m2[3] + m1[2]*m2[6]);
        m3[1] = -(m1[0]*m2[1] + m1[1]*m2[4] + m1[2]*m2[7]);
        m3[2] = -(m1[0]*m2[2] + m1[1]*m2[5] + m1[2]*m2[8]);
        
        m3[3] = -(m1[3]*m2[0] + m1[4]*m2[3] + m1[5]*m2[6]);
        m3[4] = -(m1[3]*m2[1] + m1[4]*m2[4] + m1[5]*m2[7]);
        m3[5] = -(m1[3]*m2[2] + m1[4]*m2[5] + m1[5]*m2[8]);
        
        m3[6] = -(m1[6]*m2[0] + m1[7]*m2[3] + m1[8]*m2[6]);
        m3[7] = -(m1[6]*m2[1] + m1[7]*m2[4] + m1[8]*m2[7]);
        m3[8] = -(m1[6]*m2[2] + m1[7]*m2[5] + m1[8]*m2[8]);
}

///////////////////////////////////////////////////////

void formCrossProdMat(const double *v, double *m){
        m[0] = 0.0;	m[1] = -v[2]; 	m[2] = v[1];
        m[3] = v[2];	m[4] = 0.0;	m[5] = -v[0];
        m[6] = -v[1];	m[7] = v[0];    m[8] = 0.0;        
}

void inv_mat2x2(const double *A, double *iM){
        double det = A[0]*A[3] - A[1]*A[2];
        #ifdef RSIM_SAFE
                assert(fabs(det)>1.0E-10);
        #endif
        double id = 1.0/det;
        
        iM[0] =  A[3]*id; iM[1] = -A[1]*id;
        iM[2] = -A[2]*id; iM[3] =  A[0]*id;
}

///////////////////////////////////////////////////////

void mult_mat33_vec3(const double *m, const double *v, double *op){
        op[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2];
        op[1] = m[3]*v[0] + m[4]*v[1] + m[5]*v[2];
        op[2] = m[6]*v[0] + m[7]*v[1] + m[8]*v[2];
}

///////////////////////////////////////////////////////

void mmult_mat33_vec3(const double *m, const double *v, double *op){
        op[0] = -(m[0]*v[0] + m[1]*v[1] + m[2]*v[2]);
        op[1] = -(m[3]*v[0] + m[4]*v[1] + m[5]*v[2]);
        op[2] = -(m[6]*v[0] + m[7]*v[1] + m[8]*v[2]);
}

///////////////////////////////////////////////////////

void transpose_mult_mat33_vec3(const double *m, const double& n0,const double& n1,const double& n2, double *p){
        p[0]=m[0]*n0 + m[3]*n1 + m[6]*n2;
        p[1]=m[1]*n0 + m[4]*n1 + m[7]*n2;
        p[2]=m[2]*n0 + m[5]*n1 + m[8]*n2;
}

///////////////////////////////////////////////////////

void mult_mat66_vec6(const double *I, const double *F, double *p){
        p[0]=F[0]*I[0]+F[1]*I[1]+F[2]*I[2]+F[3]*I[3]+F[4]*I[4]+F[5]*I[5];
        p[1]=F[4]*I[10]+F[0]*I[6]+F[5]*I[11]+F[1]*I[7]+F[2]*I[8]+F[3]*I[9];
        p[2]=F[0]*I[12]+F[1]*I[13]+F[2]*I[14]+F[3]*I[15]+F[4]*I[16]+F[5]*I[17];
        p[3]=F[0]*I[18]+F[1]*I[19]+F[2]*I[20]+F[3]*I[21]+F[4]*I[22]+F[5]*I[23];
        p[4]=F[0]*I[24]+F[1]*I[25]+F[2]*I[26]+F[3]*I[27]+F[4]*I[28]+F[5]*I[29];
        p[5]=F[0]*I[30]+F[1]*I[31]+F[2]*I[32]+F[3]*I[33]+F[4]*I[34]+F[5]*I[35];
}

///////////////////////////////////////////////////////

void mult_mat66_mat63(const double *I, const double *A, double *m){
        m[0]=A[0]*I[0]+A[3]*I[1]+A[12]*I[4]+A[15]*I[5]+A[6]*I[2]+A[9]*I[3];
        m[1]=A[1]*I[0]+A[10]*I[3]+A[4]*I[1]+A[13]*I[4]+A[16]*I[5]+A[7]*I[2];
        m[2]=A[2]*I[0]+A[11]*I[3]+A[14]*I[4]+A[5]*I[1]+A[17]*I[5]+A[8]*I[2];
        m[3]=A[12]*I[10]+A[15]*I[11]+A[0]*I[6]+A[3]*I[7]+A[6]*I[8]+A[9]*I[9];
        m[4]=A[13]*I[10]+A[16]*I[11]+A[1]*I[6]+A[10]*I[9]+A[4]*I[7]+A[7]*I[8];
        m[5]=A[14]*I[10]+A[17]*I[11]+A[2]*I[6]+A[11]*I[9]+A[5]*I[7]+A[8]*I[8];
        m[6]=A[0]*I[12]+A[12]*I[16]+A[15]*I[17]+A[3]*I[13]+A[6]*I[14]+A[9]*I[15];
        m[7]=A[1]*I[12]+A[10]*I[15]+A[13]*I[16]+A[16]*I[17]+A[4]*I[13]+A[7]*I[14];
        m[8]=A[11]*I[15]+A[14]*I[16]+A[17]*I[17]+A[2]*I[12]+A[5]*I[13]+A[8]*I[14];
        m[9]=A[0]*I[18]+A[12]*I[22]+A[15]*I[23]+A[3]*I[19]+A[6]*I[20]+A[9]*I[21];
        m[10]=A[1]*I[18]+A[10]*I[21]+A[13]*I[22]+A[16]*I[23]+A[4]*I[19]+A[7]*I[20];
        m[11]=A[2]*I[18]+A[11]*I[21]+A[14]*I[22]+A[17]*I[23]+A[5]*I[19]+A[8]*I[20];
        m[12]=A[0]*I[24]+A[12]*I[28]+A[15]*I[29]+A[3]*I[25]+A[6]*I[26]+A[9]*I[27];
        m[13]=A[1]*I[24]+A[10]*I[27]+A[13]*I[28]+A[16]*I[29]+A[4]*I[25]+A[7]*I[26];
        m[14]=A[11]*I[27]+A[14]*I[28]+A[17]*I[29]+A[2]*I[24]+A[5]*I[25]+A[8]*I[26];
        m[15]=A[0]*I[30]+A[12]*I[34]+A[15]*I[35]+A[3]*I[31]+A[6]*I[32]+A[9]*I[33];
        m[16]=A[1]*I[30]+A[10]*I[33]+A[13]*I[34]+A[16]*I[35]+A[4]*I[31]+A[7]*I[32];
        m[17]=A[11]*I[33]+A[14]*I[34]+A[17]*I[35]+A[2]*I[30]+A[5]*I[31]+A[8]*I[32];
}

///////////////////////////////////////////////////////

void transpose_spatial_basis_shift_vec(const double *C, const double *A, double *p){
        p[0]=A[0]*C[0]+A[1]*C[3]+A[2]*C[6];
        p[1]=A[0]*C[1]+A[1]*C[4]+A[2]*C[7];
        p[2]=A[0]*C[2]+A[1]*C[5]+A[2]*C[8];
        p[3]=A[3]*C[0]+A[4]*C[3]+A[5]*C[6];
        p[4]=A[3]*C[1]+A[4]*C[4]+A[5]*C[7];
        p[5]=A[3]*C[2]+A[4]*C[5]+A[5]*C[8];
}

///////////////////////////////////////////////////////

void transpose_basis_shift_vec3(const double *C, const double *A, double *p){
        p[0]=A[0]*C[0] + A[1]*C[3] + A[2]*C[6];
        p[1]=A[0]*C[1] + A[1]*C[4] + A[2]*C[7];
        p[2]=A[0]*C[2] + A[1]*C[5] + A[2]*C[8];
}

///////////////////////////////////////////////////////

void shift_spatial_vec(const double *r, const double *v, double *p){
        
        p[0] = v[0] + r[1]*v[5] - r[2]*v[4];
        p[1] = v[1] - r[0]*v[5] + r[2]*v[3];
        p[2] = v[2] + r[0]*v[4] - r[1]*v[3];
        p[3] = v[3];
        p[4] = v[4];
        p[5] = v[5];
}

///////////////////////////////////////////////////////

void spatial_shift_vec6(const double *F, const double *beta, double *p){ 
        p[0]=F[0]-F[4]*beta[2]+F[5]*beta[1];
        p[1]=F[1]+F[3]*beta[2]-F[5]*beta[0];
        p[2]=F[2]-F[3]*beta[1]+F[4]*beta[0];
        p[3]=F[3];
        p[4]=F[4];
        p[5]=F[5];
}

///////////////////////////////////////////////////////

void spatial_shift_vec3(const double *F, const double *beta, double *p){
        p[0]=F[2]*beta[1]-F[1]*beta[2];
        p[1]=F[0]*beta[2]-F[2]*beta[0];
        p[2]=F[1]*beta[0]-F[0]*beta[1];
        p[3]=F[0];
        p[4]=F[1];
        p[5]=F[2];
}

void form_rigid_body_transformation_matrix(     const double *x,
                                                double *m){
        for(int i=0;i<36;++i)
                m[i]=0;
        for(int i=0;i<6;++i)
                m[i+6*i] = 1.0;
                        m[4] = -x[2];           m[5] = x[1];
        m[9] = x[2];                            m[11] = -x[0];
        m[15] = -x[1];  m[16]=x[0];
}

///////////////////////////////////////////////////////

void transpose_skbeta_x_vec6(const double *beta, const double *A, double *p){
        p[0]=A[0];
        p[1]=A[1];
        p[2]=A[2];
        p[3]=A[3]+A[1]*beta[2]-A[2]*beta[1];
        p[4]=A[4]-A[0]*beta[2]+A[2]*beta[0];
        p[5]=A[5]+A[0]*beta[1]-A[1]*beta[0];
}

///////////////////////////////////////////////////////

void mult_mat36_vec6(const double *A, const double *Z, double *m){
        m[0]=A[0]*Z[0]+A[1]*Z[1]+A[2]*Z[2]+A[3]*Z[3]+A[4]*Z[4]+A[5]*Z[5];
        m[1]=A[10]*Z[4]+A[6]*Z[0]+A[11]*Z[5]+A[7]*Z[1]+A[8]*Z[2]+A[9]*Z[3];
        m[2]=A[12]*Z[0]+A[13]*Z[1]+A[14]*Z[2]+A[15]*Z[3]+A[16]*Z[4]+A[17]*Z[5];
}

///////////////////////////////////////////////////////

void mult_mat66_mat66(const double *I, const double *A, double *m){
        m[0]=A[0]*I[0]+A[12]*I[2]+A[18]*I[3]+A[24]*I[4]+A[6]*I[1]+A[30]*I[5];
        m[1]=A[1]*I[0]+A[13]*I[2]+A[19]*I[3]+A[25]*I[4]+A[7]*I[1]+A[31]*I[5];
        m[2]=A[2]*I[0]+A[14]*I[2]+A[20]*I[3]+A[26]*I[4]+A[32]*I[5]+A[8]*I[1];
        m[3]=A[3]*I[0]+A[15]*I[2]+A[21]*I[3]+A[27]*I[4]+A[33]*I[5]+A[9]*I[1];
        m[4]=A[10]*I[1]+A[16]*I[2]+A[4]*I[0]+A[22]*I[3]+A[28]*I[4]+A[34]*I[5];
        m[5]=A[11]*I[1]+A[17]*I[2]+A[5]*I[0]+A[23]*I[3]+A[29]*I[4]+A[35]*I[5];
        m[6]=A[24]*I[10]+A[30]*I[11]+A[0]*I[6]+A[12]*I[8]+A[18]*I[9]+A[6]*I[7];
        m[7]=A[25]*I[10]+A[31]*I[11]+A[1]*I[6]+A[13]*I[8]+A[19]*I[9]+A[7]*I[7];
        m[8]=A[26]*I[10]+A[32]*I[11]+A[2]*I[6]+A[14]*I[8]+A[20]*I[9]+A[8]*I[7];
        m[9]=A[27]*I[10]+A[33]*I[11]+A[3]*I[6]+A[15]*I[8]+A[21]*I[9]+A[9]*I[7];
        m[10]=A[28]*I[10]+A[34]*I[11]+A[10]*I[7]+A[16]*I[8]+A[4]*I[6]+A[22]*I[9];
        m[11]=A[29]*I[10]+A[35]*I[11]+A[11]*I[7]+A[17]*I[8]+A[5]*I[6]+A[23]*I[9];
        m[12]=A[0]*I[12]+A[12]*I[14]+A[18]*I[15]+A[24]*I[16]+A[30]*I[17]+A[6]*I[13];
        m[13]=A[1]*I[12]+A[13]*I[14]+A[19]*I[15]+A[25]*I[16]+A[31]*I[17]+A[7]*I[13];
        m[14]=A[14]*I[14]+A[2]*I[12]+A[20]*I[15]+A[26]*I[16]+A[32]*I[17]+A[8]*I[13];
        m[15]=A[15]*I[14]+A[21]*I[15]+A[27]*I[16]+A[3]*I[12]+A[33]*I[17]+A[9]*I[13];
        m[16]=A[10]*I[13]+A[16]*I[14]+A[22]*I[15]+A[28]*I[16]+A[34]*I[17]+A[4]*I[12];
        m[17]=A[11]*I[13]+A[17]*I[14]+A[23]*I[15]+A[29]*I[16]+A[35]*I[17]+A[5]*I[12];
        m[18]=A[0]*I[18]+A[12]*I[20]+A[18]*I[21]+A[24]*I[22]+A[30]*I[23]+A[6]*I[19];
        m[19]=A[1]*I[18]+A[13]*I[20]+A[19]*I[21]+A[25]*I[22]+A[31]*I[23]+A[7]*I[19];
        m[20]=A[2]*I[18]+A[14]*I[20]+A[20]*I[21]+A[26]*I[22]+A[32]*I[23]+A[8]*I[19];
        m[21]=A[15]*I[20]+A[3]*I[18]+A[21]*I[21]+A[27]*I[22]+A[33]*I[23]+A[9]*I[19];
        m[22]=A[10]*I[19]+A[16]*I[20]+A[22]*I[21]+A[28]*I[22]+A[4]*I[18]+A[34]*I[23];
        m[23]=A[11]*I[19]+A[17]*I[20]+A[23]*I[21]+A[29]*I[22]+A[35]*I[23]+A[5]*I[18];
        m[24]=A[0]*I[24]+A[12]*I[26]+A[18]*I[27]+A[24]*I[28]+A[30]*I[29]+A[6]*I[25];
        m[25]=A[1]*I[24]+A[13]*I[26]+A[19]*I[27]+A[25]*I[28]+A[31]*I[29]+A[7]*I[25];
        m[26]=A[14]*I[26]+A[2]*I[24]+A[20]*I[27]+A[26]*I[28]+A[32]*I[29]+A[8]*I[25];
        m[27]=A[15]*I[26]+A[21]*I[27]+A[27]*I[28]+A[3]*I[24]+A[33]*I[29]+A[9]*I[25];
        m[28]=A[10]*I[25]+A[16]*I[26]+A[22]*I[27]+A[28]*I[28]+A[34]*I[29]+A[4]*I[24];
        m[29]=A[11]*I[25]+A[17]*I[26]+A[23]*I[27]+A[29]*I[28]+A[35]*I[29]+A[5]*I[24];
        m[30]=A[0]*I[30]+A[12]*I[32]+A[18]*I[33]+A[24]*I[34]+A[30]*I[35]+A[6]*I[31];
        m[31]=A[1]*I[30]+A[13]*I[32]+A[19]*I[33]+A[25]*I[34]+A[31]*I[35]+A[7]*I[31];
        m[32]=A[14]*I[32]+A[2]*I[30]+A[20]*I[33]+A[26]*I[34]+A[32]*I[35]+A[8]*I[31];
        m[33]=A[15]*I[32]+A[21]*I[33]+A[27]*I[34]+A[3]*I[30]+A[33]*I[35]+A[9]*I[31];
        m[34]=A[10]*I[31]+A[16]*I[32]+A[22]*I[33]+A[28]*I[34]+A[34]*I[35]+A[4]*I[30];
        m[35]=A[11]*I[31]+A[17]*I[32]+A[23]*I[33]+A[29]*I[34]+A[35]*I[35]+A[5]*I[30];
}

void mmult_mat66_mat66(const double *I, const double *A, double *m){
        m[0]=-(A[0]*I[0]+A[12]*I[2]+A[18]*I[3]+A[24]*I[4]+A[6]*I[1]+A[30]*I[5]);
        m[1]=-(A[1]*I[0]+A[13]*I[2]+A[19]*I[3]+A[25]*I[4]+A[7]*I[1]+A[31]*I[5]);
        m[2]=-(A[2]*I[0]+A[14]*I[2]+A[20]*I[3]+A[26]*I[4]+A[32]*I[5]+A[8]*I[1]);
        m[3]=-(A[3]*I[0]+A[15]*I[2]+A[21]*I[3]+A[27]*I[4]+A[33]*I[5]+A[9]*I[1]);
        m[4]=-(A[10]*I[1]+A[16]*I[2]+A[4]*I[0]+A[22]*I[3]+A[28]*I[4]+A[34]*I[5]);
        m[5]=-(A[11]*I[1]+A[17]*I[2]+A[5]*I[0]+A[23]*I[3]+A[29]*I[4]+A[35]*I[5]);
        m[6]=-(A[24]*I[10]+A[30]*I[11]+A[0]*I[6]+A[12]*I[8]+A[18]*I[9]+A[6]*I[7]);
        m[7]=-(A[25]*I[10]+A[31]*I[11]+A[1]*I[6]+A[13]*I[8]+A[19]*I[9]+A[7]*I[7]);
        m[8]=-(A[26]*I[10]+A[32]*I[11]+A[2]*I[6]+A[14]*I[8]+A[20]*I[9]+A[8]*I[7]);
        m[9]=-(A[27]*I[10]+A[33]*I[11]+A[3]*I[6]+A[15]*I[8]+A[21]*I[9]+A[9]*I[7]);
        m[10]=-(A[28]*I[10]+A[34]*I[11]+A[10]*I[7]+A[16]*I[8]+A[4]*I[6]+A[22]*I[9]);
        m[11]=-(A[29]*I[10]+A[35]*I[11]+A[11]*I[7]+A[17]*I[8]+A[5]*I[6]+A[23]*I[9]);
        m[12]=-(A[0]*I[12]+A[12]*I[14]+A[18]*I[15]+A[24]*I[16]+A[30]*I[17]+A[6]*I[13]);
        m[13]=-(A[1]*I[12]+A[13]*I[14]+A[19]*I[15]+A[25]*I[16]+A[31]*I[17]+A[7]*I[13]);
        m[14]=-(A[14]*I[14]+A[2]*I[12]+A[20]*I[15]+A[26]*I[16]+A[32]*I[17]+A[8]*I[13]);
        m[15]=-(A[15]*I[14]+A[21]*I[15]+A[27]*I[16]+A[3]*I[12]+A[33]*I[17]+A[9]*I[13]);
        m[16]=-(A[10]*I[13]+A[16]*I[14]+A[22]*I[15]+A[28]*I[16]+A[34]*I[17]+A[4]*I[12]);
        m[17]=-(A[11]*I[13]+A[17]*I[14]+A[23]*I[15]+A[29]*I[16]+A[35]*I[17]+A[5]*I[12]);
        m[18]=-(A[0]*I[18]+A[12]*I[20]+A[18]*I[21]+A[24]*I[22]+A[30]*I[23]+A[6]*I[19]);
        m[19]=-(A[1]*I[18]+A[13]*I[20]+A[19]*I[21]+A[25]*I[22]+A[31]*I[23]+A[7]*I[19]);
        m[20]=-(A[2]*I[18]+A[14]*I[20]+A[20]*I[21]+A[26]*I[22]+A[32]*I[23]+A[8]*I[19]);
        m[21]=-(A[15]*I[20]+A[3]*I[18]+A[21]*I[21]+A[27]*I[22]+A[33]*I[23]+A[9]*I[19]);
        m[22]=-(A[10]*I[19]+A[16]*I[20]+A[22]*I[21]+A[28]*I[22]+A[4]*I[18]+A[34]*I[23]);
        m[23]=-(A[11]*I[19]+A[17]*I[20]+A[23]*I[21]+A[29]*I[22]+A[35]*I[23]+A[5]*I[18]);
        m[24]=-(A[0]*I[24]+A[12]*I[26]+A[18]*I[27]+A[24]*I[28]+A[30]*I[29]+A[6]*I[25]);
        m[25]=-(A[1]*I[24]+A[13]*I[26]+A[19]*I[27]+A[25]*I[28]+A[31]*I[29]+A[7]*I[25]);
        m[26]=-(A[14]*I[26]+A[2]*I[24]+A[20]*I[27]+A[26]*I[28]+A[32]*I[29]+A[8]*I[25]);
        m[27]=-(A[15]*I[26]+A[21]*I[27]+A[27]*I[28]+A[3]*I[24]+A[33]*I[29]+A[9]*I[25]);
        m[28]=-(A[10]*I[25]+A[16]*I[26]+A[22]*I[27]+A[28]*I[28]+A[34]*I[29]+A[4]*I[24]);
        m[29]=-(A[11]*I[25]+A[17]*I[26]+A[23]*I[27]+A[29]*I[28]+A[35]*I[29]+A[5]*I[24]);
        m[30]=-(A[0]*I[30]+A[12]*I[32]+A[18]*I[33]+A[24]*I[34]+A[30]*I[35]+A[6]*I[31]);
        m[31]=-(A[1]*I[30]+A[13]*I[32]+A[19]*I[33]+A[25]*I[34]+A[31]*I[35]+A[7]*I[31]);
        m[32]=-(A[14]*I[32]+A[2]*I[30]+A[20]*I[33]+A[26]*I[34]+A[32]*I[35]+A[8]*I[31]);
        m[33]=-(A[15]*I[32]+A[21]*I[33]+A[27]*I[34]+A[3]*I[30]+A[33]*I[35]+A[9]*I[31]);
        m[34]=-(A[10]*I[31]+A[16]*I[32]+A[22]*I[33]+A[28]*I[34]+A[34]*I[35]+A[4]*I[30]);
        m[35]=-(A[11]*I[31]+A[17]*I[32]+A[23]*I[33]+A[29]*I[34]+A[35]*I[35]+A[5]*I[30]);
}

///////////////////////////////////////////////////////

void mult_mat63_mat33(const double *A, const double *I, double *m){
        m[0]=A[0]*I[0]+A[1]*I[3]+A[2]*I[6];
        m[1]=A[0]*I[1]+A[1]*I[4]+A[2]*I[7];
        m[2]=A[0]*I[2]+A[1]*I[5]+A[2]*I[8];
        m[3]=A[3]*I[0]+A[4]*I[3]+A[5]*I[6];
        m[4]=A[3]*I[1]+A[4]*I[4]+A[5]*I[7];
        m[5]=A[3]*I[2]+A[4]*I[5]+A[5]*I[8];
        m[6]=A[6]*I[0]+A[7]*I[3]+A[8]*I[6];
        m[7]=A[6]*I[1]+A[7]*I[4]+A[8]*I[7];
        m[8]=A[6]*I[2]+A[7]*I[5]+A[8]*I[8];
        m[9]=A[10]*I[3]+A[11]*I[6]+A[9]*I[0];
        m[10]=A[10]*I[4]+A[11]*I[7]+A[9]*I[1];
        m[11]=A[10]*I[5]+A[11]*I[8]+A[9]*I[2];
        m[12]=A[12]*I[0]+A[13]*I[3]+A[14]*I[6];
        m[13]=A[12]*I[1]+A[13]*I[4]+A[14]*I[7];
        m[14]=A[12]*I[2]+A[13]*I[5]+A[14]*I[8];
        m[15]=A[15]*I[0]+A[16]*I[3]+A[17]*I[6];
        m[16]=A[15]*I[1]+A[16]*I[4]+A[17]*I[7];
        m[17]=A[15]*I[2]+A[16]*I[5]+A[17]*I[8];
}

///////////////////////////////////////////////////////

void mult_mat63_vec3(const double *A, const double *Z, double *m){
        m[0]=A[0]*Z[0]+A[1]*Z[1]+A[2]*Z[2];
        m[1]=A[3]*Z[0]+A[4]*Z[1]+A[5]*Z[2];
        m[2]=A[6]*Z[0]+A[7]*Z[1]+A[8]*Z[2];
        m[3]=A[10]*Z[1]+A[11]*Z[2]+A[9]*Z[0];
        m[4]=A[12]*Z[0]+A[13]*Z[1]+A[14]*Z[2];
        m[5]=A[15]*Z[0]+A[16]*Z[1]+A[17]*Z[2];
}

///////////////////////////////////////////////////////

void mult_mat33_mat36(const double *I, const double *A, double *m){
        m[0]=A[0]*I[0]+A[12]*I[2]+A[6]*I[1];
        m[1]=A[1]*I[0]+A[13]*I[2]+A[7]*I[1];
        m[2]=A[2]*I[0]+A[14]*I[2]+A[8]*I[1];
        m[3]=A[3]*I[0]+A[15]*I[2]+A[9]*I[1];
        m[4]=A[10]*I[1]+A[16]*I[2]+A[4]*I[0];
        m[5]=A[11]*I[1]+A[17]*I[2]+A[5]*I[0];
        m[6]=A[0]*I[3]+A[12]*I[5]+A[6]*I[4];
        m[7]=A[1]*I[3]+A[13]*I[5]+A[7]*I[4];
        m[8]=A[2]*I[3]+A[14]*I[5]+A[8]*I[4];
        m[9]=A[3]*I[3]+A[15]*I[5]+A[9]*I[4];
        m[10]=A[10]*I[4]+A[16]*I[5]+A[4]*I[3];
        m[11]=A[11]*I[4]+A[17]*I[5]+A[5]*I[3];
        m[12]=A[0]*I[6]+A[12]*I[8]+A[6]*I[7];
        m[13]=A[1]*I[6]+A[13]*I[8]+A[7]*I[7];
        m[14]=A[2]*I[6]+A[14]*I[8]+A[8]*I[7];
        m[15]=A[3]*I[6]+A[15]*I[8]+A[9]*I[7];
        m[16]=A[10]*I[7]+A[16]*I[8]+A[4]*I[6];
        m[17]=A[11]*I[7]+A[17]*I[8]+A[5]*I[6];
}

void mmult_mat33_mat36(const double *I, const double *A, double *m){
        m[0]=-(A[0]*I[0]+A[12]*I[2]+A[6]*I[1]);
        m[1]=-(A[1]*I[0]+A[13]*I[2]+A[7]*I[1]);
        m[2]=-(A[2]*I[0]+A[14]*I[2]+A[8]*I[1]);
        m[3]=-(A[3]*I[0]+A[15]*I[2]+A[9]*I[1]);
        m[4]=-(A[10]*I[1]+A[16]*I[2]+A[4]*I[0]);
        m[5]=-(A[11]*I[1]+A[17]*I[2]+A[5]*I[0]);
        m[6]=-(A[0]*I[3]+A[12]*I[5]+A[6]*I[4]);
        m[7]=-(A[1]*I[3]+A[13]*I[5]+A[7]*I[4]);
        m[8]=-(A[2]*I[3]+A[14]*I[5]+A[8]*I[4]);
        m[9]=-(A[3]*I[3]+A[15]*I[5]+A[9]*I[4]);
        m[10]=-(A[10]*I[4]+A[16]*I[5]+A[4]*I[3]);
        m[11]=-(A[11]*I[4]+A[17]*I[5]+A[5]*I[3]);
        m[12]=-(A[0]*I[6]+A[12]*I[8]+A[6]*I[7]);
        m[13]=-(A[1]*I[6]+A[13]*I[8]+A[7]*I[7]);
        m[14]=-(A[2]*I[6]+A[14]*I[8]+A[8]*I[7]);
        m[15]=-(A[3]*I[6]+A[15]*I[8]+A[9]*I[7]);
        m[16]=-(A[10]*I[7]+A[16]*I[8]+A[4]*I[6]);
        m[17]=-(A[11]*I[7]+A[17]*I[8]+A[5]*I[6]);
}

void transpose_skbeta_x_mat63(const double *beta, const double *A, double *m){
        m[0]=A[0];
        m[1]=A[1];
        m[2]=A[2];
        m[3]=A[3];
        m[4]=A[4];
        m[5]=A[5];
        m[6]=A[6];
        m[7]=A[7];
        m[8]=A[8];
        m[9]=A[9]+A[3]*beta[2]-A[6]*beta[1];
        m[10]=A[10]+A[4]*beta[2]-A[7]*beta[1];
        m[11]=A[11]+A[5]*beta[2]-A[8]*beta[1];
        m[12]=A[12]-A[0]*beta[2]+A[6]*beta[0];
        m[13]=A[13]-A[1]*beta[2]+A[7]*beta[0];
        m[14]=A[14]-A[2]*beta[2]+A[8]*beta[0];
        m[15]=A[15]+A[0]*beta[1]-A[3]*beta[0];
        m[16]=A[16]+A[1]*beta[1]-A[4]*beta[0];
        m[17]=A[17]+A[2]*beta[1]-A[5]*beta[0];
}

void transpose_skbeta_x_mat66(const double *beta, const double *I, double *m){
        m[0]=I[0];
        m[1]=I[1];
        m[2]=I[2];
        m[3]=I[3];
        m[4]=I[4];
        m[5]=I[5];
        m[6]=I[6];
        m[7]=I[7];
        m[8]=I[8];
        m[9]=I[9];
        m[10]=I[10];
        m[11]=I[11];
        m[12]=I[12];
        m[13]=I[13];
        m[14]=I[14];
        m[15]=I[15];
        m[16]=I[16];
        m[17]=I[17];
        m[18]=I[18]-I[12]*beta[1]+I[6]*beta[2];
        m[19]=I[19]-I[13]*beta[1]+I[7]*beta[2];
        m[20]=I[20]-I[14]*beta[1]+I[8]*beta[2];
        m[21]=I[21]-I[15]*beta[1]+I[9]*beta[2];
        m[22]=I[22]+I[10]*beta[2]-I[16]*beta[1];
        m[23]=I[23]+I[11]*beta[2]-I[17]*beta[1];
        m[24]=I[24]-I[0]*beta[2]+I[12]*beta[0];
        m[25]=I[25]-I[1]*beta[2]+I[13]*beta[0];
        m[26]=I[26]+I[14]*beta[0]-I[2]*beta[2];
        m[27]=I[27]+I[15]*beta[0]-I[3]*beta[2];
        m[28]=I[28]+I[16]*beta[0]-I[4]*beta[2];
        m[29]=I[29]+I[17]*beta[0]-I[5]*beta[2];
        m[30]=I[30]+I[0]*beta[1]-I[6]*beta[0];
        m[31]=I[31]+I[1]*beta[1]-I[7]*beta[0];
        m[32]=I[32]+I[2]*beta[1]-I[8]*beta[0];
        m[33]=I[33]+I[3]*beta[1]-I[9]*beta[0];
        m[34]=I[34]-I[10]*beta[0]+I[4]*beta[1];
        m[35]=I[35]-I[11]*beta[0]+I[5]*beta[1];
}

void dot6(const double *v1, const double *v2, double *v){
        v[0] = v1[0]*v2[0];
        v[0] += v1[1]*v2[1];
        v[0] += v1[2]*v2[2];
        v[0] += v1[3]*v2[3];
        v[0] += v1[4]*v2[4];
        v[0] += v1[5]*v2[5];        
}

///////////////////////////////////////////////////////

} // namespace FastOps

}// namespace RSIM